#include "KinectListener.h"
#include <iostream>

//handler is global so the callbacks can forward calls to it
KinectHandler *m_handler;

//constructor
KinectListener::KinectListener(KinectHandler *handler) {
	m_handler = handler;
}

void XnVSecondaryFilter::Update(const XnVMultipleHands& hands) {
	// Copy all the hands
	hands.Clone(m_LocalCopy);

	const XnVHandPointContext* pPrimary = hands.GetPrimaryContext();
	if (pPrimary == NULL)
	{
		// There are no points at all.
		return;
	}

	XnUInt32 nPrimary = pPrimary->nID;

	if (hands.GetContext(m_nNewPrimary) == 0)
	{
		// The secondary point we considered primary for the subtree is no longer available.
		m_nNewPrimary = 0;
	}

	// If we don't remember a secondary point as primary, or that secondary just became the primary for
	// the entire tree, try to locate another secondary.
	if (m_nNewPrimary == 0 || m_nNewPrimary == nPrimary)
	{
		// local primary unavailable
		for (XnVMultipleHands::ConstIterator iter = hands.begin(); iter != hands.end(); ++iter)
		{
			if (iter.IsActive() && (*iter)->nID != nPrimary)
			{
				// Found a point that is active, but is not the primary. This will be the primary
				// for the subtree
				m_nNewPrimary = (*iter)->nID;
				break;
			}
		}
	}
	// Adjust the hand list with our chosen primary for the subtree
	m_LocalCopy.ReassignPrimary(m_nNewPrimary);
}

// Received a new message
void XnVSecondaryFilter::Update(XnVMessage* pMessage) {
	// Deal with point message (calls Update(const XnVMultipleHands&)). This will fill the local copy.
	XnVPointControl::Update(pMessage);

	// Replace the points part of the message with the local copy.
	GenerateReplaced(pMessage, m_LocalCopy);

	// If there are no active entries (which means - no points), a new point will be looked for next time.
	if (m_LocalCopy.ActiveEntries() == 0) {
		m_nNewPrimary = 0;
	}
}

//destructor
KinectListener::~KinectListener() {
	delete m_timer;
	delete m_ctx;
	delete m_handsGenerator;
	delete m_depthGenerator;
	delete m_sessionManager;
	delete m_waveDetector1;
	delete m_waveDetector2;
	delete m_pd1;
	delete m_pd2;
	delete m_secFilter;
}

//-----------------------------------------------------------------------------
// Callbacks
//-----------------------------------------------------------------------------

//callback for session start
void XN_CALLBACK_TYPE SessionStart(const XnPoint3D& ptFocusPoint, void* UserCxt) {}

//callback for session end
void XN_CALLBACK_TYPE SessionEnd(void* UserCxt) {}

//callback for push gesture
void XN_CALLBACK_TYPE OnPushCB(XnFloat fVelocity, XnFloat fAngle, void* cxt) {
	//emit the push signal with either 1 or 2 depending on which context (primary/secondary) evoked this
	if(std::strcmp((XnChar*)cxt, "Primary Push") == 0) {
		m_handler->receivePush(1);
	}
	else {
		m_handler->receivePush(2);
	}
}

//called after a hand has stabilized following a push
void XN_CALLBACK_TYPE OnStableCB(XnFloat fVelocity, void* cxt) {}

//called after a wave gesture
void XN_CALLBACK_TYPE OnWaveCB(void* cxt) {}

//called when either hand changes position
void XN_CALLBACK_TYPE OnPointUpdateCB(const XnVHandPointContext* pContext, void* cxt) {
	m_handler->receivePointUpdate(pContext->ptPosition.X, pContext->ptPosition.Y, pContext->ptPosition.Z, pContext->nID);
}

//called when a new hand point is detected
void XN_CALLBACK_TYPE OnPointCreateCB(const XnVHandPointContext *pContext, void *cxt)
{
	m_handler->receivePointCreate(pContext->nID);
}

//called when a hand point is lost
void XN_CALLBACK_TYPE OnPointDestroyCB(XnUInt32 nID, void *cxt)
{
	m_handler->receivePointDestroy(nID);
}

//----------------------------------------------------------------------------

//returns false if something failed to initialize
bool KinectListener::init() {
	XnStatus rc = XN_STATUS_OK;

	//create context and the two generators
	m_ctx = new xn::Context();
	m_handsGenerator = new xn::HandsGenerator();
	m_depthGenerator = new xn::DepthGenerator();

	//attempt to initialize from XML file, return false if failed
	rc = m_ctx->InitFromXmlFile(SAMPLE_XML_PATH);
	if(rc != XN_STATUS_OK) {
		return false;
	}

	//attempt to register the generator nodes
	rc = m_ctx->FindExistingNode(XN_NODE_TYPE_DEPTH, *m_depthGenerator);
	if(rc != XN_STATUS_OK) {
		return false;
	}
	rc = m_ctx->FindExistingNode(XN_NODE_TYPE_HANDS, *m_handsGenerator);
	if(rc != XN_STATUS_OK) {
		return false;
	}

	//create a new session or this context
	m_sessionManager = new XnVSessionManager();
	if(NULL == m_sessionManager)
		return false;
	
	//try to initialize this session, specifying click or wave as focus gestures
	rc = m_sessionManager->Initialize(m_ctx, "Click,Wave", "RaiseHand");
	if(rc != XN_STATUS_OK) {
		return false;
	}

	//register the session
	m_sessionManager->RegisterSession(NULL, SessionStart, SessionEnd, NULL);

	//create push detectors and secondary filter
	m_pd1 = new XnVPushDetector();
	m_pd2 = new XnVPushDetector();
	m_secFilter = new XnVSecondaryFilter();

	//register callbacks for both push detectors
	m_pd1->RegisterPush("Primary Push", OnPushCB);
 	m_pd1->RegisterStabilized("Primary Stable", OnStableCB);
 	m_pd2->RegisterPush("Secondary Push", OnPushCB);
 	m_pd2->RegisterStabilized("Secondary Stable", OnStableCB);

	//create wave/point update detector and register callbacks
	m_waveDetector1 = new XnVWaveDetector();
	m_waveDetector1->RegisterWave("Primary Wave", OnWaveCB);
	m_waveDetector1->RegisterPointUpdate("Point Update", OnPointUpdateCB);
	m_waveDetector1->RegisterPointCreate("Point Created", OnPointCreateCB);
	m_waveDetector1->RegisterPointDestroy("Point Destroyed", OnPointDestroyCB);
	m_waveDetector2 = new XnVWaveDetector();
	m_waveDetector2->RegisterWave("Secondary Wave", OnWaveCB);

	// Connect tree:
	// A SecondaryFilter is connected to the SessionManager,
	// one PushDetector is connected to the SessionManager (will work on the primary point),
	// and the other PushDetector is connected to the SecondaryFitler (will work on the secondary point)
	m_sessionManager->AddListener(m_pd1);
	m_sessionManager->AddListener(m_waveDetector1);
 	m_sessionManager->AddListener(m_secFilter);
	m_secFilter->AddListener(m_pd2);
	m_secFilter->AddListener(m_waveDetector2);

	//return true since everything succeeded
	return true;
}

//overload of QThread's run method
void KinectListener::run() {
	//loop infinitely until initialization succeeds (will fail if Kinect is off or not connected)
	while(!this->init()) {}

	//create the timer, set up its connection and start it
	m_timer = new QTimer();
	QObject::connect(m_timer, SIGNAL(timeout()), this, SLOT(updateKinect()), Qt::DirectConnection);
    m_timer->start(15);

	//let the GUI know the Kinect has been found
	emit signalKinectConnected();

	//thread rests here until program is terminated
	exec();

	//shutdown and stop
	m_timer->stop();
	m_ctx->Shutdown();
}

//called whenever timer ticks
void KinectListener::updateKinect() {
	// Read next available data
	m_ctx->WaitOneUpdateAll(*m_depthGenerator);
	// Process the data
	m_sessionManager->Update(m_ctx);
}

//returns false if the context has any sort of error
bool KinectListener::checkDeviceConnection() {
	return m_ctx->GetGlobalErrorState() == 0;
}
